#pragma once
#ifndef MYSLAM_VISUAL_ODOMETRY_H
#define MYSLAM_VISUAL_ODOMETRY_H

#include "myslam/backend.h"
#include "myslam/common_include.h"
#include "myslam/dataset.h"
#include "myslam/transfer.h"
#include "myslam/frontend.h"
#include "myslam/viewer.h"


namespace myslam {

/**
 * VO 对外接口
 */
class VisualOdometry {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<VisualOdometry> Ptr;

    /// constructor with config file
    VisualOdometry(std::string &config_path);
    ~VisualOdometry();
    /**
     * do initialization things before run
     * @return true if success
     */
    bool Init();

    /**
     * start vo in the dataset
     */
    void Run();

    /**
     * Make a step forward in dataset
     */
    bool Step();

    /// 获取前端状态
    FrontendStatus GetFrontendStatus() const { return frontend_->GetStatus(); }

    void Visual_Odometry_Stop();
    void Visual_Odometry_Update(cv::Mat* left_pic,cv::Mat* right_pic);

    void SetTransfer(std::shared_ptr<Transfer> transfer) { transfer_ = transfer; }
    
   private:
        std::thread visual_odometry_thread_;
        std::mutex data_mutex_;
        std::condition_variable visual_odometry_update_;
        std::atomic<bool> visual_odometry_running_;

    bool inited_ = false;
    bool success_flag = false;
    std::string config_file_path_;

    Frontend::Ptr frontend_ = nullptr;
    Backend::Ptr backend_ = nullptr;
    Map::Ptr map_ = nullptr;
    Viewer::Ptr viewer_ = nullptr;
    std::shared_ptr<Transfer> transfer_ = nullptr;

    // dataset
    Dataset::Ptr dataset_ = nullptr;
};
}  // namespace myslam

#endif  // MYSLAM_VISUAL_ODOMETRY_H
